import numpy as np
from bresenham import bresenham

# NO ROS utility in this scripts

THRESHOLD_FREE_P = 0.3
THRESHOLD_OCC_P = 0.9

class GridMap:
    """
    This grid map is static and easy, without considering the sub map.
    And assume the laser is capable to find the max range of the whole environment
    """
    def __init__(self, x_lim, y_lim, resolution, p):
        # store local variables
        self.x_lim = x_lim
        self.y_lim = y_lim
        self.resolution = resolution
        x = np.arange(x_lim[0], x_lim[1], resolution)
        y = np.arange(y_lim[0], y_lim[1], resolution)

        # Construct the log odds matrix
        self.l = np.full(shape=(len(x), len(y)), fill_value=self.log_odds(p))


    @staticmethod
    def log_odds(p):
        """
        Compute a log odds of probability(s) p
        Note that odds is :
            __p__
            1 - p
        """
        assert p >= 0 and p <= 1, "A probability must in [0, 1]"
        return np.log(p/ (1 - p))

    def retrieve_p(self):
        """
        Convenience class for retrieving a probaility from log odds
        """
        return 1 - 1 / (1 + np.exp(self.l))

    @property
    def shape(self):
        return self.l.shape

    def discretize(self, x_rel, y_rel):
        """
        Compute the coordinate of the points in the Inertial Frame
        """
        assert x_rel >= self.x_lim[0] and x_rel <= self.x_lim[1]
        assert y_rel >= self.y_lim[0] and y_rel <= self.y_lim[1]
        print(x_rel == float('inf') or y_rel == float('inf'))
        x_cnt = (x_rel - self.x_lim[0]) // self.resolution
        y_cnt = (y_rel - self.y_lim[0]) // self.resolution
        return  x_cnt, y_cnt

    def update(self, x, y, p):
        """
        Update the map using inverse_sensor_model
        Here directly add the log_odds
        """
        x = int(x)
        y = int(y)
        self.l[x][y] += self.log_odds(p)

    def calc_MLE(self):
        """
        Calculate the MLE of the log_odds matrix
        """
        mask_free = self.l < self.log_odds(THRESHOLD_FREE_P)
        self.l[mask_free] = self.log_odds(0.01)
        mask_occ = self.l > self.log_odds(THRESHOLD_OCC_P)
        self.l[mask_occ] = self.log_odds(0.99)
        mask_no_knowledge = (1 - (mask_free | mask_occ)) == 1
        self.l[mask_no_knowledge] = self.log_odds(0.5)


if __name__ == "__main__":  
    test = GridMap([-3, 3], [-3, 3], 1.5, 0.99)
    test.calc_MLE()
